import cv2
import numpy as np
import glob
import time
import sys

def nothing(x):
    pass

class Calibration():

    RedMi = "mi"

    Usb = "usb"

    def __init__(self,Cam = None):
        """
        Calibration类：获取相机信息。
        - Calibration(Cam = None)
        - Cam:使用相机的名称（可用：Cam.RedMi 或者 Cam.Usb），若输入为空，默认使用RedMi
        """
        if Cam == "usb":
            self.mtx = np.array([[962.48516891,0,294.3380071],[0,961.25896096,208.45140198],[0,0,1,]],dtype = "double")
            self.dist = np.array([[-0.42943663,0.44220127,0.00072504,-0.0007345,-0.25149261]])

        else:
            self.dist = np.array([9.72393379e-02,-5.32960429e-01,-3.30763901e-04,-5.01014326e-04,9.50551478e-01])
            # self.mtx = np.array([[2.99819372e+03,0.00000000e+00,1.99648432e+03],
            #                     [0.00000000e+00,3.00218939e+03,1.14086425e+03],
            #                     [0.00000000e+00,0.00000000e+00,1.00000000e+00]],dtype = "double")
            mtx1 = [[3025.19510044585,  0      ,2003.18506868456],
                    [0,      3024.91969587358,  1127.71184430812],
                    [0,                 0,                     1]]
            self.mtx = np.array(mtx1,dtype="double")

class Time():

    def __init__(self):
        self.time = None

    def Tik(self):

        if self.time is None:
            self.time = time.time()

        else:
            UseTime = time.time() - self.time
            self.time = time.time()
            return UseTime

class Tools:

    USE_PIC = "USEPIC" 
    """-在处理中使用图片。"""

    USE_CAM = "USECAM" 
    """-在处理中使用摄像输入。"""
    
    def Process(self,flag = None, input = None):
        """
        process：对输入的HSV的双阈值进行调参
        - process(flag, input) \n
        - flag："USEPIC" 或者 "USECAM" \n
        - input：img_path（图片路径）或者 cap（cap对象）
        - return : 返回一个顺序为[up_h,low_h,up_s,low_s,up_v,low_v]的数组
        """
        if flag != "USEPIC" and flag != "USECAM":
            print("[ERROR] You flag fucking wrong!")
            return 

        cv2.namedWindow("color", cv2.WINDOW_NORMAL)  # 调整窗口
        cv2.resizeWindow("color", 300, 200)

        cv2.createTrackbar("Top_Hue", "color", 0, 255, nothing)  # 色调
        cv2.createTrackbar("low_Hue", "color", 0, 255, nothing)
        cv2.createTrackbar("Top_Saturation", "color", 0, 255, nothing)  # 饱和度
        cv2.createTrackbar("low_Saturation", "color", 0, 255, nothing)
        cv2.createTrackbar("Top_Value", "color", 0, 255, nothing)  # 亮度
        cv2.createTrackbar("low_Value", "color", 0, 255, nothing)

        light_bgr = cv2.imread("./tools/light.jpg", 1)
        light_hsv = cv2.cvtColor(light_bgr, cv2.COLOR_BGR2HSV_FULL)
        # 亮条数据 + 预处理

        if flag == "USEPIC":
            
            img = cv2.imread(input)
            img_hsv = cv2.cvtColor(img, cv2.COLOR_BGR2HSV_FULL)
                # (摄像头读取,打开) 动态转hsv

        while 1:
            
            if flag == "USECAM":
                suc, img = input.read()
                # (摄像头读取,打开)
                img_hsv = cv2.cvtColor(img, cv2.COLOR_BGR2HSV_FULL)
                # (摄像头读取,打开) 动态转hsv

            up_s = cv2.getTrackbarPos("Top_Saturation", "color")
            up_v = cv2.getTrackbarPos("Top_Value", "color")
            up_h = cv2.getTrackbarPos("Top_Hue", "color")
            low_h = cv2.getTrackbarPos("low_Hue", "color")
            low_s = cv2.getTrackbarPos("low_Saturation", "color")
            low_v = cv2.getTrackbarPos("low_Value", "color")

            # ________________遮罩处理_______________:
            low = np.array([low_h, low_s, low_v])
            high = np.array([up_h, up_s, up_v])
            # 双阀值数组

            mask = cv2.inRange(img_hsv, low, high)
            mask_light = cv2.inRange(light_hsv, low, high)
            light = cv2.bitwise_and(light_bgr, light_bgr, mask=mask_light)

            cv2.imshow("color",light)
            mask = cv2.resize(mask, [1280,720])
            cv2.imshow("mask",mask)
            img = cv2.resize(img, [1280,720])
            cv2.imshow("input",img)

            key = cv2.waitKey(1)
            if key == ord("q"):
                process_data = [up_h,low_h,up_s,low_s,up_v,low_v]
                print("[tools]:up_h,low_h,up_s,low_s,up_v,low_v = ",process_data)
                return process_data

    def Calibrate(self):
        """
        calibrate:使用opencv进行数据标定
        - 标定图片文件放置于cal_img文件夹
        """

        # 找棋盘格角点标定并且写入文件
        # 设置寻找亚像素角点的参数，采用的停止准则是最大循环次数30和最大误差容限0.001
        criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.0001)  # 阈值
        # 棋盘格模板规格（横竖交点数）
        w = 8  
        h = 11  
        # 世界坐标系中的棋盘格点,例如(0,0,0), (1,0,0), (2,0,0) ....,(8,5,0)，去掉Z坐标，记为二维矩阵
        objp = np.zeros((w * h, 3), np.float32)
        objp[:, :2] = np.mgrid[0:w, 0:h].T.reshape(-1, 2)
        objp = objp * 30  # 棋盘方块边长30 mm
        # 世界坐标系

        # 储存棋盘格角点的世界坐标和图像坐标对
        objpoints = []  # 在世界坐标系中的三维点
        imgpoints = []  # 在图像平面的二维点

        images = glob.glob('.\\cal_imgs\\*')  # 拍摄的十几张棋盘图片所在目录

        i = 1
        for fname in images:
            img = cv2.imread(fname)
            # 获取画面中心点
            h1, w1 = img.shape[0], img.shape[1]
            gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
            u, v = img.shape[:2]
            print(u, v)
            # 找到棋盘格角点
            ret, corners = cv2.findChessboardCorners(gray, (w, h), None)
            # 如果找到足够点对，将其存储起来
            if ret:
                print("i:", i)
                i = i+1
                # 对检测到的角点作进一步的优化计算，可使角点的精度达到亚像素级别
                cv2.cornerSubPix(gray, corners, (11, 11), (-1, -1), criteria)
                objpoints.append(objp)
                imgpoints.append(corners)
                # 将角点在图像上显示
                cv2.drawChessboardCorners(img, (w, h), corners, ret)
                cv2.namedWindow('findCorners', cv2.WINDOW_NORMAL)
                cv2.resizeWindow('findCorners', 640, 480)
                cv2.imshow('findCorners', img)
                cv2.waitKey(200)

        cv2.destroyAllWindows()
        #  标定

        print('正在计算')
        ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(objpoints, imgpoints, gray.shape[::-1], None, None)

        # cv_file = cv2.FileStorage("D:/Maxhuk/desktop/1/1.yaml", cv2.FILE_STORAGE_WRITE)
        # cv_file.write("camera_matrix", mtx)
        # cv_file.write("dist_coeff", dist)
        # # 请注意，*释放*不会关闭（）FileStorage对象
        # cv_file.release()

        # print("ret:", ret)
        print("mtx:\n", mtx)      # 内参数矩阵
        print("dist畸变值:\n", dist)   # 畸变系数   
        # # distortion cofficients = (k_1,k_2,p_1,p_2,k_3)
        # print("rvecs旋转（向量）外参:\n", rvecs)   # 旋转向量  # 外参数
        # print("tvecs平移（向量）外参:\n", tvecs)  # 平移向量  # 外参数
        # newcameramtx, roi = cv2.getOptimalNewCameraMatrix(mtx, dist, (u, v), 0, (u, v))
        # print('newcameramtx 外参', newcameramtx)
    
    def SortPoints(approx):
        """
        sort_points：对四边形轮的4点进行点辨别。\n
        - sort_points(approx) \n
        - approx：四边形的轮廓。
        - return：为一个顺序为[上，下，左，右]的numpy数组。
        """

        if len(approx) != 4:
            print("[tools]:This function only supports rectangle approx!You fuck up obviously!")
            return 
        
        center_x = np.mean(approx[:, 0, 0])
        center_y = np.mean(approx[:, 0, 1])
        center_point = [center_x, center_y]

        list_x = []
        list_y = []

        points = approx[:, 0, :]
        center_point = np.array([center_x, center_y])
        diff = points - center_point
        list_x = diff[:, 0].tolist()
        list_y = diff[:, 1].tolist()

        left_index = list_x.index(min(list_x))
        right_index = list_x.index(max(list_x))
        up_index =  list_y.index(min(list_y))
        down_index = list_y.index(max(list_y))

        # print("上：",approx[up_index])
        # print("下：",approx[down_index])
        # print("左：",approx[left_index])
        # print("右：",approx[right_index])

        return np.array([approx[up_index],approx[down_index],approx[left_index],approx[right_index]],dtype = "double"),center_point

    def PointLabel(points,img):
        """
        point_label：绘制四边形4点
        - point_label(points,img)
        - points：输入一个表示4点的(4,2)的numpy数组
        - img：绘制的图像
        - return : img(绘制好的图像)
        """
        cv2.putText(img,"up",points[0].astype(np.int32),cv2.FONT_HERSHEY_SIMPLEX,10,(0, 255, 0),5)
        cv2.putText(img,"down",points[1].astype(np.int32),cv2.FONT_HERSHEY_SIMPLEX,10,(0, 255, 0),5)
        cv2.putText(img,"left",points[2].astype(np.int32),cv2.FONT_HERSHEY_SIMPLEX,10,(0, 255, 0),5)
        cv2.putText(img,"right",points[3].astype(np.int32),cv2.FONT_HERSHEY_SIMPLEX,10,(0, 255, 0),5)

        return img

    def ShotImg(self):
        """
        point_label：保存相机照片
        - 相片保存至shot文件夹
        - 按s保存，按q退出
        """
        # cap = cv2.VideoCapture(0)
        cap = cv2.VideoCapture(0, cv2.CAP_DSHOW)  # 快速读取
        # cap.set(3, 1280)  # width=1920
        # cap.set(4, 720)  # height=1080
        save_path = ".//shot//"
        num = 0
        while True:
            suc,img = cap.read()

            img2show = cv2.resize(img,(1280,960))
            cv2.imshow("img",img2show)
            key = cv2.waitKey(1)

            if key == ord('s'):
                path = save_path + str(num) + ".jpg"
                cv2.imwrite(path,img)
                num = num + 1
                print("[tools]:Saving ",num," image.")

            if key == ord('q'):
                print("[tools]:Shot_img Quitting...")
                return 0
            
    def Cont2Rect(slef,counter):
        rect = cv2.boundingRect(counter)
        x, y, w, h = rect
        # CenterPoint = (int(x+w/2),int(y+h/2))
        return (x, y), (x + w, y + h)
    

# def main():
#     path = "./fct/1.png"
#     t = tools()
#     t.process(tools.USE_PIC,input=path)

# main()